#include <iostream>
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
#include <string.h>
#include <libgen.h>
#include <signal.h>
#include "mjpg_streamer.h"
#include "Camera.h"

using namespace std;
using namespace Robot;

int main(int argc, char *argv[])
{
    minIni* ini = new minIni("config.ini");
    Image* rgb_output = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);

    LinuxCamera::GetInstance()->Initialize(0);
    LinuxCamera::GetInstance()->SetCameraSettings(CameraSettings());    // set default
    LinuxCamera::GetInstance()->LoadINISettings(ini);                   // load from ini

    mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);
    ColorFinder* ball_finder = new ColorFinder();
    ball_finder->LoadINISettings(ini);
    httpd::ball_finder = ball_finder;

    ColorFinder* red_finder = new ColorFinder(0, 15, 45, 0, 0.3, 50.0);
    red_finder->LoadINISettings(ini, "RED");
    httpd::red_finder = red_finder;

    ColorFinder* yellow_finder = new ColorFinder(60, 15, 45, 0, 0.3, 50.0);
    yellow_finder->LoadINISettings(ini, "YELLOW");
    httpd::yellow_finder = yellow_finder;

    ColorFinder* blue_finder = new ColorFinder(225, 15, 45, 0, 0.3, 50.0);
    blue_finder->LoadINISettings(ini, "BLUE");
    httpd::blue_finder = blue_finder;

    httpd::ini = ini;

    while(1){
        Point2D ball_pos, red_pos, yellow_pos, blue_pos;
        LinuxCamera::GetInstance()->CaptureFrame();
        memcpy(rgb_output->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageSize);
        ball_pos = ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
        red_pos = red_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
        yellow_pos = yellow_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
        blue_pos = blue_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);

        unsigned char r, g, b;
        for(int i = 0; i < rgb_output->m_NumberOfPixels; i++)
        {
            r = 0; g = 0; b = 0;
            if(ball_finder->m_result->m_ImageData[i] == 1)
            {
                r = 255;
                g = 128;
                b = 0;
            }
            if(red_finder->m_result->m_ImageData[i] == 1)
            {
                if(ball_finder->m_result->m_ImageData[i] == 1)
                {
                    r = 0;
                    g = 255;
                    b = 0;
                }
                else
                {
                    r = 255;
                    g = 0;
                    b = 0;
                }
            }
            if(yellow_finder->m_result->m_ImageData[i] == 1)
            {
                if(ball_finder->m_result->m_ImageData[i] == 1)
                {
                    r = 0;
                    g = 255;
                    b = 0;
                }
                else
                {
                    r = 255;
                    g = 255;
                    b = 0;
                }
            }
            if(blue_finder->m_result->m_ImageData[i] == 1)
            {
                if(ball_finder->m_result->m_ImageData[i] == 1)
                {
                    r = 0;
                    g = 255;
                    b = 0;
                }
                else
                {
                    r = 0;
                    g = 0;
                    b = 255;
                }
            }

            if(r > 0 || g > 0 || b > 0)
            {
                rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 0] = r;
                rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 1] = g;
                rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 2] = b;
            }
        }
        streamer->send_image(rgb_output);
    }
    return 0;
}
